
#include "Collider.h"

namespace p2 {

Collider::~Collider()
{
}

bool Collider::collide(Element* a, Element* b)
{
	ConvexPolygon* polyA = ConvexPolygon::cast(a);
	if (polyA) {
		ConvexPolygon* polyB = ConvexPolygon::cast(b);
		if (polyB) {
			return collidePolygonPolygon(polyA, polyB);
		} else {
			Disc* discB = Disc::cast(b);
			if (discB) {
				return collidePolygonDisc(polyA, discB);
			}
		}
	} else {
		Disc* discA = Disc::cast(a);
		if (discA) {
			Disc* discB = Disc::cast(b);
			if (discB) {
				return collideDiscDisc(discA, discB);
			} else {
				ConvexPolygon* polyB = ConvexPolygon::cast(b);
				if (polyB) {
					return collidePolygonDisc(polyB, discA);
				}
			}
		}
	}
	return false;
}

/*
 * REF: http://www.wildbunny.co.uk/blog/2011/04/20/collision-detection-for-dummies/
 * 'It turns out that when the Minkowski Difference .. of two shapes
 * contains the origin, the two shapes are intersecting.'
 */
bool Collider::collidePolygonPolygon(ConvexPolygon* a, ConvexPolygon* b)
{
	real sumInvMass = a->getInvMass() + b->getInvMass();
	if (sumInvMass == 0.f) {
		return false;
	}
	real penetrationDepth = -Math<real>::MAX_REAL; // init depth
	ConvexPolygon* vBody = NULL;
	int32 vIndex = NULL_INDEX;
	ConvexPolygon* eBody = NULL;
	int32 eIndex[2] = { NULL_INDEX, NULL_INDEX };
	/*
	 * face of A, vertices of B
	 */
	uint32 sizeA = a->getWorldVertices().size();
	for (uint32 i = 0; i < sizeA; i++) {
		uint32 wV0i = i;
		uint32 wV1i = (i + 1) % sizeA;

		Vector2<real> &V0 = a->getWorldVertices()[wV0i];
		Vector2<real> &V1 = a->getWorldVertices()[wV1i];
		Vector2<real> &N = a->getWorldNormals()[wV0i];

		uint32 svi[2];
		int ss = b->getSupportingVertices(-N, svi);
		for (int j = 0; j < ss; j++) {
			Vector2<real> mfp0(b->getWorldVertices()[svi[j]] - V0);
			Vector2<real> mfp1(b->getWorldVertices()[svi[j]] - V1);

			real faceDistance = mfp0.dot(N);

			Vector2<real> p = Vector2<real>::projectPointOntoEdgeClamped(Vector2<real>::ZERO, mfp0, mfp1);

			// get distance
			real dsign = Math<real>::Sign(faceDistance);
			real dist = p.length() * dsign;
			if (dist > penetrationDepth) {
				vBody = b;
				eBody = a;
				vIndex = svi[j];
				eIndex[0] = wV0i;
				eIndex[1] = wV1i;
				penetrationDepth = dist;
			}
		}
	}
	/*
	 * face of B, vertices of A
	 */
	uint32 sizeB = b->getWorldVertices().size();
	for (uint32 i = 0; i < sizeB; i++) {
		uint32 wV0i = i;
		uint32 wV1i = (i + 1) % sizeB;

		Vector2<real> &V0 = b->getWorldVertices()[wV0i];
		Vector2<real> &V1 = b->getWorldVertices()[wV1i];
		Vector2<real> &N = b->getWorldNormals()[wV0i];

		uint32 svi[2];
		int ss = a->getSupportingVertices(-N, svi);
		for (int j = 0; j < ss; j++) {
			Vector2<real> mfp0(V0 - a->getWorldVertices()[svi[j]]);
			Vector2<real> mfp1(V1 - a->getWorldVertices()[svi[j]]);

			real faceDistance = -mfp0.dot(N);

			Vector2<real> p = Vector2<real>::projectPointOntoEdgeClamped(Vector2<real>::ZERO, mfp0, mfp1);

			// get distance
			real dsign = Math<real>::Sign(faceDistance);
			real dist = p.length() * dsign;
			if (dist > penetrationDepth) {
				vBody = a;
				eBody = b;
				vIndex = svi[j];
				eIndex[0] = wV0i;
				eIndex[1] = wV1i;
				penetrationDepth = dist;
			}
		}
	}

	if (vBody) {
		/*
		 * find full contact set
		 * => determine contact edge on the edge body
		 */
		Vector2<real> &cebV0 = eBody->getWorldVertices()[eIndex[0]];
		Vector2<real> &cebV1 = eBody->getWorldVertices()[eIndex[1]];
		Vector2<real> const& cebN = eBody->getWorldNormals()[eIndex[0]];
		/*
		 * determine contact edge on the vertex body
		 */
		int32 pvi = (vIndex - 1 + vBody->getWorldVertices().size())
			% vBody->getWorldVertices().size();
		int32 nvi = (vIndex + 1) % vBody->getWorldVertices().size();
		Vector2<real> const& lastNormal = vBody->getWorldNormals()[pvi];
		Vector2<real> const& thisNormal = vBody->getWorldNormals()[vIndex];
		/*
		 * vertex body edge is most parallel to the contact edge
		 */
		real lastDP = lastNormal.dot(-cebN);
		real thisDP = thisNormal.dot(-cebN);
		uint32 cvbVi0, cvbVi1;
		if (lastDP > thisDP) {
			/*
			 * correct vertex body edge has vertices last and this
			 */
			cvbVi0 = pvi;
			cvbVi1 = vIndex;
		} else {
			/*
			 * correct vertex body edge has vertices this and next
			 */
			cvbVi0 = vIndex;
			cvbVi1 = nvi;
		}
		Vector2<real> &cvbV0 = vBody->getWorldVertices()[cvbVi0];
		Vector2<real> &cvbV1 = vBody->getWorldVertices()[cvbVi1];

		/*
		 * now we have the collision edges, project each collision vertex
		 * of each edge onto the other edge making sure to clamp to each edge space
		 */
		Vector2<real> vBodyPoints[2], eBodyPoints[2];
		eBodyPoints[0] = Vector2<real>::projectPointOntoEdgeClamped(cvbV0, cebV0, cebV1);
		vBodyPoints[0] = Vector2<real>::projectPointOntoEdgeClamped(eBodyPoints[0], cvbV0, cvbV1);
		eBodyPoints[1] = Vector2<real>::projectPointOntoEdgeClamped(cvbV1, cebV0, cebV1);
		vBodyPoints[1] = Vector2<real>::projectPointOntoEdgeClamped(eBodyPoints[1], cvbV0, cvbV1);
		/*
		 * create the contacts
		 */
		_contacts.push_back(Contact(vBody, eBody, vBodyPoints, eBodyPoints, -cebN));
		return true;
	}
	return false;
}

bool Collider::collidePolygonDisc(ConvexPolygon* a, Disc* b)
{
	Vector2<real> bWorldPosition = b->getWorldPosition();
	Vector2<real> cLocal = a->getTransform().transformIntoLocalSpace(
			a->getOwner()->getTransform().transformIntoLocalSpace(bWorldPosition));
	int32 normalIndex = 0;
	real separation = -Math<real>::MAX_REAL;
	/*
	 * find polygon edge most facing disc
	 */
	std::vector<Vector2<real> > const& aLocalVertices = a->getLocalVertices();
	std::vector<Vector2<real> > const& aLocalNormals = a->getLocalNormals();
	int32 sizeV = a->getWorldVertices().size();
	for (int32 i = 0; i < sizeV; ++i) {
		real s = Vector2<real>::dot(aLocalNormals[i], cLocal - aLocalVertices[i]);
		if (s > separation) {
			separation = s;
			normalIndex = i;
		}
	}
	/*
	 * find point on closest polygon edge closest to disc
	 */
	std::vector<Vector2<real> > const& aWorldVertices = a->getWorldVertices();
	std::vector<Vector2<real> > const& aWorldNormals = a->getWorldNormals();
	Vector2<real> const& v0 = aWorldVertices[normalIndex];
	Vector2<real> const& v1 = aWorldVertices[(normalIndex + 1) % sizeV];
	Vector2<real> p0 = Vector2<real>::projectPointOntoEdgeClamped(bWorldPosition, v0, v1);
	Vector2<real> dxpb = p0 - bWorldPosition;
	if ( dxpb.squaredLength() > b->getRadius()*b->getRadius() ) {
		return false;
	}
	/*
	 * swap normal if disc centre is inside the polygon
	 */
	Vector2<real> const& faceNormal = aWorldNormals[normalIndex];
	if (faceNormal.dot(dxpb) > 0.f) {
		dxpb = -dxpb;
	}
	dxpb.normalise();
    _contacts.push_back(Contact(b, a, bWorldPosition + dxpb * b->getRadius(), p0, dxpb));
	return true;
}

bool Collider::collideDiscDisc(Disc* a, Disc* b)
{
	Vector2<real> dist = b->getWorldPosition() - a->getWorldPosition();
	if ( dist.squaredLength() > (a->getRadius()+b->getRadius())*(a->getRadius()+b->getRadius()) ) {
		return false;
	}
	real distLength = dist.length();
	Vector2<real> normal;
	if (distLength == 0.f) {
		normal = Vector2<real>::UNIT_Y;
	} else {
		normal = dist / distLength;
	}
	Vector2<real> pa = a->getWorldPosition() + normal * a->getRadius();
	Vector2<real> pb = b->getWorldPosition() - normal * b->getRadius();
    _contacts.push_back(Contact(a, b, pa, pb, normal));
	return true;
}

#if ( DRAW_CONTACTS )
void Collider::renderContacts()
{
	::glPointSize(5.f);
	::glBegin (GL_POINTS);
	p2ContactList::iterator iter = _contacts.begin();
	for (; iter != _contacts.end(); ++iter) {
		Contact& contact = (*iter);
		for (uint32 pi = 0; pi < 2; ++pi) {
			if (contact._dist[pi] < 0.f) {
				::glColor3f(1.f, 0.f, 0.f);
			} else {
				::glColor3f(0.f, 0.f, 1.f);
			}
			::glVertex2f(contact._pa[pi].X(), contact._pa[pi].Y());
			::glVertex2f(contact._pb[pi].X(), contact._pb[pi].Y());
		}
	}
	::glEnd();
	::glPointSize(1.f);
	::glBegin (GL_LINES);
	for (iter = _contacts.begin(); iter != _contacts.end(); ++iter) {
		Contact& contact = (*iter);
		for (uint32 pi = 0; pi < 2; ++pi) {
			Vector2<real> penetration(contact._normal * contact._dist[pi]);
			if (contact._dist[pi] < 0.f) {
				::glColor3f(1.f, 0.f, 0.f);
			} else {
				::glColor3f(0.f, 0.f, 1.f);
			}
			::glVertex2f(contact._pa[pi].X(), contact._pa[pi].Y());
			::glVertex2f(contact._pa[pi].X() + penetration.X(),
				contact._pa[pi].Y() + penetration.Y());
			::glVertex2f(contact._pb[pi].X(), contact._pb[pi].Y());
			::glVertex2f(contact._pb[pi].X() - penetration.X(),
				contact._pb[pi].Y() - penetration.Y());
		}
	}
	::glEnd();
}
#endif	//DRAW_CONTACTS

}
